Test name: Open Loop with LQR focus on travel
Variables changed: Q for travel set to 100, others 1
Expected behaviour: Optimal travel trajectory should be followed much closer, no drift
Results: Almost as expected, some constant deviation from reference travel at the end.